#include "mc_foc.h"
#include "mc_math.h"
#include "mc_control.h"
#include "stdbool.h"

extern PidHandle_t id_pid;
extern PidHandle_t iq_pid;


//return sector 1-6 based on alpha beta
//1:110->110 2:110->010 3:010->011 4:011->001 5:001->101 6:101->100
uint8_t sector_judgment(AlphaBeta_t input)
{
    const uint8_t sector_trans[6] = {2, 6, 1, 4, 3, 5};
    bool new_a = (input.beta > 0);
    bool new_b = (((SQRT_3 * input.alpha) >> 15 ) - input.beta) > 0;
    bool new_c = (-(( (SQRT_3 * input.alpha) >> 15 ) + input.beta)) > 0;
    uint8_t sector_n = new_a + 2 * new_b + 4 * new_c;

    return sector_trans[sector_n - 1];
}


//pwm value output
//input:Ua,Ub after pid, uvw: time duty output
/**
  * @brief  cal the pwm duty of H bridge driver from Ua Ub and sector of rotor
  * @param  input: Ualpha and Ubeta after unti-park 
  * @param  tim_arr: Timer's arr value, the max and min value is cal from it
  * @param  sector: rotor's virtual secotr
  * @param  uvw: pwm duty output uvw.sa, sb, sc
  * @retval 0
  */
int8_t pwm_duty_out(AlphaBeta_t input, uint16_t tim_arr, uint8_t sector, Uvw_t* uvw)
{
  const int32_t Ts = 65536;
  int32_t tx, ty;
  int32_t sa, sb, sc;
  if(sector > 6)
  {
    return -1;
  }
  switch (sector)
  {
  case sector_one:
    tx =  (SQRT_3*(((SQRT_3_DIV2*input.alpha)>>15) - (input.beta / 2)))>>15;
    ty =  (SQRT_3*input.beta)>>15;
    sa =  (Ts - tx - ty) / 4;
    sb =  (Ts + tx - ty) / 4;
    sc =  (Ts + tx + ty) / 4;
    break;
  case sector_two:  
    tx =  (SQRT_3* ( (-((SQRT_3_DIV2*input.alpha)>>15)) + (input.beta / 2) )) >> 15;
    ty =  (SQRT_3* ( ( ((SQRT_3_DIV2*input.alpha)>>15)) + (input.beta / 2) )) >> 15;
    sa =  (Ts + tx - ty) / 4;
    sb =  (Ts - tx - ty) / 4;
    sc =  (Ts + tx + ty) / 4;
    break;
  case sector_three:
    tx =  (SQRT_3*input.beta)>>15;
    ty = -(SQRT_3* ( ( ((SQRT_3_DIV2*input.alpha)>>15)) + (input.beta / 2) )) >> 15;
    sa =  (Ts + tx + ty) / 4;
    sb =  (Ts - tx - ty) / 4;
    sc =  (Ts + tx - ty) / 4;
    break;
  case sector_four:
    tx = -(SQRT_3*input.beta)>>15;
    ty =  (SQRT_3* ( (-((SQRT_3_DIV2*input.alpha)>>15)) + (input.beta / 2) )) >> 15;
    sa =  (Ts + tx + ty) / 4;
    sb =  (Ts + tx - ty) / 4;
    sc =  (Ts - tx - ty) / 4;
    break;
  case sector_five:
    tx = -(SQRT_3* ( ( ((SQRT_3_DIV2*input.alpha)>>15)) + (input.beta / 2) )) >> 15;
    ty = -(SQRT_3* ( (-((SQRT_3_DIV2*input.alpha)>>15)) + (input.beta / 2) )) >> 15;
    sa =  (Ts + tx - ty) / 4;
    sb =  (Ts + tx + ty) / 4;
    sc =  (Ts - tx - ty) / 4;
    break;
  case sector_six:
    tx =  (SQRT_3* ( ( ((SQRT_3_DIV2*input.alpha)>>15)) + (input.beta / 2) )) >> 15;
    ty = -(SQRT_3*input.beta)>>15;
    sa =  (Ts - tx - ty) / 4;
    sb =  (Ts + tx + ty) / 4;
    sc =  (Ts + tx - ty) / 4;
    break;
  default:
    break;
  }
  uvw->ta = (tim_arr/2) - (((sa - 16384) * (tim_arr/2))>>(15 - 1));
  uvw->tb = (tim_arr/2) - (((sb - 16384) * (tim_arr/2))>>(15 - 1));
  uvw->tc = (tim_arr/2) - (((sc - 16384) * (tim_arr/2))>>(15 - 1));
  return 0;
}



/**
  * @brief  open loop control of motor drv
  * @param  motor: motor structour
  * @retval null
  */
void virtual_foc(Motor_t *motor)
{
    uint8_t sector;
    Qd_t qd = {0};
    qd.d = 0;
    qd.q = 3000; // max current is 32767
    AlphaBeta_t alpha_beta = {0};
    Uvw_t uvw = {0};
    static int32_t theta = 0;
    if (theta > 32767)
    {
        theta -= 65535;
    }
    theta += 12u;

    //
    alpha_beta = rev_park_conv(qd, theta);
    sector = sector_judgment(alpha_beta);
    pwm_duty_out(alpha_beta, 3600, sector, &uvw);
    motor->mc_foc_param.sa = uvw.ta;
    motor->mc_foc_param.sb = uvw.tb;
    motor->mc_foc_param.sc = uvw.tc;
    set_pwm(motor);
}

/**
  * @brief  close loop control of motor drv
  * @param  motor: motor structour
  * @retval null
  */
void foc_control(Motor_t *motor)
{
    uint8_t sector_act;
    Ab_t ab_act = {0}; 
    static Qd_t qd_act_i = {0};
    static Qd_t qd_act_u = {0};
    Qd_t qd_ref_i = {0}; 
    AlphaBeta_t alpha_beta_act_i = {0};
    AlphaBeta_t alpha_beta_act_u = {0}; 
    Uvw_t uvw_act = {0};
    int32_t theta_act = motor->encoder_content.e_angle_act;
    get_tres_phase_current(motor);
    //clark and park translate
    ab_act.a = motor->mc_foc_param.ia_norm;
    ab_act.b = motor->mc_foc_param.ib_norm;
    alpha_beta_act_i = clarke_conv(ab_act);
    qd_act_i = park_conv(alpha_beta_act_i, theta_act);
    qd_act_i.q = low_pass_filter(qd_act_i.q, motor->mc_foc_param.idq.q, 3276);//coe = 0.1f
    qd_act_i.d = 0;
    motor->mc_foc_param.idq.d = qd_act_i.d;
    motor->mc_foc_param.idq.q = qd_act_i.q;
    // pid ontroller
    qd_ref_i.d = motor->mc_foc_param.idq_ref.d;
    qd_ref_i.q = motor->mc_foc_param.idq_ref.q;
    qd_act_u.d = pi_controller(&id_pid, (qd_ref_i.d - qd_act_i.d));
    qd_act_u.q = pi_controller(&iq_pid, (qd_ref_i.q - qd_act_i.q));
    motor->mc_foc_param.udq.d = qd_act_u.d;
    motor->mc_foc_param.udq.q = qd_act_u.q;

    alpha_beta_act_u = rev_park_conv(qd_act_u, theta_act);
    sector_act = sector_judgment(alpha_beta_act_u);
    motor->encoder_content.sector_act = sector_act;
    pwm_duty_out(alpha_beta_act_u, 3600, sector_act, &uvw_act);
    motor->mc_foc_param.sa = uvw_act.ta;
    motor->mc_foc_param.sb = uvw_act.tb;
    motor->mc_foc_param.sc = uvw_act.tc;
    set_pwm(motor);
}





